#include <iostream>
#include <string>

#include "shape_approx.h"

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;

class Tester
{
public:
	Tester()
	{
		count_total = 0;
		count_passed = 0;
	}

	~Tester()
	{
		ShowResults();
	}

	void Assert(string assert_name, bool value)
	{
		count_total ++;
		if (!value)
		{
			cerr << assert_name << " failed" << endl;
		}
		else
		{
			count_passed ++;
		}
	}

	void ShowResults()
	{
		cout << "Passed " << count_passed << "/" << count_total << " tests." << endl;
	}

private:
	int count_total;
	int count_passed;
};

void testNeghtbourPoints(Tester &tester)
{
	Matrix m = Matrix(1, 1);

	tester.Assert("No Neighbours",
		get_neighbour_points(m, MatrixElement(0, 0)) == vector<MatrixElement>()
		);

	m = Matrix(3, 3);

	tester.Assert("Corner case",
		get_neighbour_points(m, MatrixElement(0, 0)).size() == 3
		);

	tester.Assert("Side case",
		get_neighbour_points(m, MatrixElement(1, 0)).size() == 5
		);

	tester.Assert("No border case",
		get_neighbour_points(m, MatrixElement(1, 1)).size() == 8
		);
}

void testFillBackground(Tester &tester)
{
	const int side = 5;
	int inputData[][side] = {
		{0, 0, 0, 0, 0},
		{1, 0, 0, 0, 0},
		{1, 0, 1, 1, 1},
		{1, 0, 1, 0, 1},
		{0, 0, 1, 1, 0}
	};

	int resultData[][side] = {
		{-1, -1, -1, -1, -1},
		{1, -1, -1, -1, -1},
		{1, -1, 1, 1, 1},
		{1, -1, 1, 0, 1},
		{-1, -1, 1, 1, 0}
	};

	Matrix input = Matrix(side, side);
	Matrix result = Matrix(side, side);

	for(int row = 0; row < side; row ++)
		for(int col = 0; col < side; col ++)
		{
			input.at(row, col) = inputData[row][col];
			result.at(row, col) = resultData[row][col];
		}

	fill_background(input, MatrixElement());

	tester.Assert("Filled background", input == result);
}

void testSegment(Tester &tester)
{
	const int side = 5;
	int inputData[][side] = {
		{-1, -1, -1, -1, -1},
		{1, -1, -1, -1, -1},
		{1, -1, 1, 1, 1},
		{1, -1, 1, 0, 1},
		{-1, -1, 1, 1, 0}
	};

	int resultData[][side] = {
		{-1, -1, -1, -1, -1},
		{10, -1, -1, -1, -1},
		{10, -1, 11, 11, 11},
		{10, -1, 11, 11, 11},
		{-1, -1, 11, 11, 11}
	};

	vector<pair<int, MatrixElement>> resultSegmentsData;
	resultSegmentsData.push_back(make_pair(10, MatrixElement(1, 0)));
	resultSegmentsData.push_back(make_pair(11, MatrixElement(2, 2)));

	Matrix input = Matrix(side, side);
	Matrix result = Matrix(side, side);

	for(int row = 0; row < side; row ++)
		for(int col = 0; col < side; col ++)
		{
			input.at(row, col) = inputData[row][col];
			result.at(row, col) = resultData[row][col];
		}

	vector<pair<int, MatrixElement>> segments_data = segment_shapes(input);
	
	tester.Assert("Segment shapes matrix", input == result);

	tester.Assert("Segment shapes result", segments_data == resultSegmentsData);
}

void testDelate(Tester &tester)
{
	const int side = 5;
	int inputData[][side] = {
		{-1, -1, -1, 10, -1},
		{11, -1, -1, -1, 10},
		{11, -1, -1, -1, -1},
		{11, -1, 12, 12, -1},
		{-1, -1, 12, 12, -1}
	};

	int resultData[][side] = {
		{11, -1, 10, 10, 10},
		{11, -1, 10, 10, 10},
		{11, 12, 12, 12, -1},
		{11, 12, 12, 12, -1},
		{-1, 12, 12, 12, -1}
	};

	vector<pair<int, MatrixElement>> inputSegmentsData;
	inputSegmentsData.push_back(make_pair(10, MatrixElement(0, 3)));
	inputSegmentsData.push_back(make_pair(11, MatrixElement(1, 0)));
	inputSegmentsData.push_back(make_pair(12, MatrixElement(3, 2)));

	vector<pair<int, MatrixElement>> resultSegmentsData;
	resultSegmentsData.push_back(make_pair(10, MatrixElement(0, 2)));
	resultSegmentsData.push_back(make_pair(11, MatrixElement(0, 0)));
	resultSegmentsData.push_back(make_pair(12, MatrixElement(2, 1)));

	Matrix input = Matrix(side, side);
	Matrix result = Matrix(side, side);

	for(int row = 0; row < side; row ++)
		for(int col = 0; col < side; col ++)
		{
			input.at(row, col) = inputData[row][col];
			result.at(row, col) = resultData[row][col];
		}

	delate(input, inputSegmentsData);
	
	tester.Assert("Dilate shapes matrix", input == result);

	tester.Assert("Dilate shapes data", inputSegmentsData == resultSegmentsData);
}

void testBasicShapeSearch(Tester &tester)
{
	const int side = 5;
	int inputData[][side] = {
		{11, -1, 10, 10, 10},
		{11, -1, 10, 10, 10},
		{11, 12, 12, 12, -1},
		{11, 12, 12, 12, -1},
		{-1, 12, 12, 12, -1}
	};

	vector<pair<int, MatrixElement>> inputSegmentsData;
	inputSegmentsData.push_back(make_pair(10, MatrixElement(0, 2)));
	inputSegmentsData.push_back(make_pair(11, MatrixElement(0, 0)));
	inputSegmentsData.push_back(make_pair(12, MatrixElement(2, 1)));

	Matrix input = Matrix(side, side);

	for(int row = 0; row < side; row ++)
		for(int col = 0; col < side; col ++)
		{
			input.at(row, col) = inputData[row][col];
		}

	vector<Shape> expectedResult;
	Shape s1;
	s1.points.push_back(Point(2, 0));
	s1.points.push_back(Point(3, 0));
	s1.points.push_back(Point(4, 0));
	s1.points.push_back(Point(4, 1));
	s1.points.push_back(Point(3, 1));
	s1.points.push_back(Point(2, 1));
	Shape s2;
	s2.points.push_back(Point(1, 2));
	s2.points.push_back(Point(2, 2));
	s2.points.push_back(Point(3, 2));
	s2.points.push_back(Point(3, 3));
	s2.points.push_back(Point(3, 4));
	s2.points.push_back(Point(2, 4));
	s2.points.push_back(Point(1, 4));
	s2.points.push_back(Point(1, 3));

	expectedResult.push_back(s1);
	expectedResult.push_back(s2);

	vector<Shape> result = find_basic_shapes(input, inputSegmentsData);
	
	tester.Assert("Find basic shape", result == expectedResult);
}

void testRemoveTrivialVertexes(Tester &tester)
{
	vector<Shape> input;

	{
		Shape s1;
		s1.points.push_back(Point(2, 0));
		s1.points.push_back(Point(3, 0));
		s1.points.push_back(Point(4, 0));
		s1.points.push_back(Point(4, 1));
		s1.points.push_back(Point(3, 1));
		s1.points.push_back(Point(2, 1));
		Shape s2;
		s2.points.push_back(Point(1, 2));
		s2.points.push_back(Point(2, 2));
		s2.points.push_back(Point(3, 2));
		s2.points.push_back(Point(3, 3));
		s2.points.push_back(Point(3, 4));
		s2.points.push_back(Point(2, 4));
		s2.points.push_back(Point(1, 4));
		s2.points.push_back(Point(1, 3));

		input.push_back(s1);
		input.push_back(s2);
	}

	vector<Shape> expectedResult;
	{
		Shape s1;
		s1.points.push_back(Point(2, 0));
		s1.points.push_back(Point(4, 0));
		s1.points.push_back(Point(4, 1));
		s1.points.push_back(Point(2, 1));
		Shape s2;
		s2.points.push_back(Point(1, 2));
		s2.points.push_back(Point(3, 2));
		s2.points.push_back(Point(3, 4));
		s2.points.push_back(Point(1, 4));

		expectedResult.push_back(s1);
		expectedResult.push_back(s2);
	}


	vector<Shape> result = remove_trivial_vertexes(input);
	
	tester.Assert("Remove trivial vertexes", result == expectedResult);
}

void unitTests()
{
	Tester tester;

	testNeghtbourPoints(tester);
	testFillBackground(tester);
	testSegment(tester);
	testDelate(tester);
	testBasicShapeSearch(tester);
	testRemoveTrivialVertexes(tester);
}

void testImageProcessing()
{
	//cv::Mat image_g = cv::imread("test-level-mask.bmp");
	cv::Mat image = cv::imread("test-level-mask.bmp");
	cv::Mat greyscale;

	//cv::cvtColor(image_g, image, CV_GRAY2RGB);
	cv::cvtColor(image, greyscale, CV_RGB2GRAY);

	Matrix m(greyscale.cols, greyscale.rows);

	for(int row=0; row<m.Height(); row++)
		for(int col=0; col<m.Width(); col++)
		{
			m.at(row, col) = greyscale.at<unsigned char>(row, col) > 64 ? 0 : 1;
		}

	vector<Shape> shapes = approximate(find_shapes(m), 4);

	int points = 0;
	int shapesCount = 0;

	for(int i=0; i<(int)shapes.size(); i++)
	{
		Shape s = shapes[i];
		cv::Scalar shapeColor = cv::Scalar(rand() % 255, rand() % 255, rand() % 255);

		int points_num = s.points.size();
		cv::Point *p = new cv::Point[points_num];

		for(int v=0; v<3; v++)
		{
			points++;
			p[v] = cv::Point((int)s.points[v].x, (int)s.points[v].y);
		}

		cv::fillConvexPoly(image, p, points_num, shapeColor);
		shapesCount++;

		delete[] p;

		/*for(int v=0; v<(int)s.points.size(); v++)
		{
			points++;

			Point p1 = s.points[v];
			Point p2 = s.points[(v + 1) % s.points.size()];

			cv::line(image, cv::Point((int)p1.x, (int)p1.y), cv::Point((int)p2.x, (int)p2.y), shapeColor);

			cv::circle(image, cv::Point((int)p1.x, (int)p1.y), 1, shapeColor, 2);
			cv::circle(image, cv::Point((int)p2.x, (int)p2.y), 1, shapeColor, 2);
		}*/
	}

	cout << "Total points: " << points << endl;
	cout << "Total shapes: " << shapesCount << endl;

	cv::imwrite("result.png", image);
}

int main()
{
	unitTests();
	testImageProcessing();

	return 0;
}
